/*******************************************************************************

  Robot Toolkit ++ (RTK++)

  Copyright (c) 2007-2014 Shuhui Bu <bushuhui@nwpu.edu.cn>
    http://www.adv-ci.com

  ----------------------------------------------------------------------------

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.

*******************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "base/utils/utils.h"

#include "AHRS_mini.h"

using namespace pi;


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

///
/// \enum AHRS_FrameType
/// \brief The AHRS_FrameType enum
///
/// This enum indicate frame type, AHRS or Sensor data
///
enum AHRS_FrameType {
    AHRS_FRAME_AHRS,                        ///< AHRS frame
    AHRS_FRAME_SENSOR                       ///< Sensor data frame
};

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

void AHRS_Frame::print(void)
{
    printf("IMU: yaw = %12f, pitch = %12f, roll  = %12f\n", yaw, pitch, roll);
    printf("     alt = %12f, temp  = %12f, press = %12f\n", alt, temp, press);
    printf("     imu_ps = %5d, crc = 0x%02x (%d), tm = %lld\n", imu_ps, crc_a, correct, tm_a);
    printf("SEN: Ax = %6d, Ay = %6d, Az = %6d\n", Ax, Ay, Az);
    printf("     Gx = %6d, Gy = %6d, Gz = %6d\n", Gx, Gy, Gz);
    printf("     Mx = %6d, My = %6d, Mz = %6d\n", Mx, My, Mz);
    printf("     crc = 0x%02x (%d), tm = %lld\n", crc_s, correct, tm_s);

    printf("     Ax = %12f, Ay = %12f, Az = %12f\n", fA[0], fA[1], fA[2]);
    printf("     Gx = %12f, Gy = %12f, Gz = %12f\n", fG[0], fG[1], fG[2]);
    printf("     Mx = %12f, My = %12f, Mz = %12f\n", fM[0], fM[1], fM[2]);

    printf("\n");
}

void AHRS_Frame::clear(void)
{
    yaw     = 0;
    pitch   = 0;
    roll    = 0;
    alt     = 0;
    temp    = 0;
    press   = 0;
    imu_ps  = 0;
    crc_a   = 0;
    crcv_a  = 0;
    tm_a    = 0;

    Ax      = 0;
    Ay      = 0;
    Az      = 0;
    Gx      = 0;
    Gy      = 0;
    Gz      = 0;
    Mx      = 0;
    My      = 0;
    Mz      = 0;
    crc_s   = 0;
    crcv_s  = 0;
    tm_s    = 0;

    tm      = 0;
    correct = 0;
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

AHRS_Mini::AHRS_Mini()
{
}

AHRS_Mini::AHRS_Mini(const std::string& port_name)
{
    uart.port_name = port_name;
    AHRS_Mini();
}

AHRS_Mini::~AHRS_Mini()
{
    stop();
}


int AHRS_Mini::setUART(const std::string& port_name)
{
    uart.port_name = port_name;

    return 0;
}

int AHRS_Mini::begin(void)
{
    // open UART port
    if( 0 != uart.open() ) {
        dbg_pe("ERR: can not open port: %s\n", uart.port_name.c_str());
        return -1;
    }

    // start receiving thread
    start();

    return 0;
}

void AHRS_Mini::stop(void)
{
    if( isRunning() ) {
        pi::Thread::stop();
        join(10);
    }
}

void AHRS_Mini::threadFunc()
{
    uint8_t         buf[64];
    uint8_t         ub;

    int             sta;
    int             frame_len;
    int             ir;

    int             j;


    sta = 0;
    while( !shouldStop() ) {
        // read one byte
        j = 0;
        while ( j<1 ) j = uart.read(&ub, 1);

        if( sta == 0 ) {
            if( ub == 0xA5 ) sta = 1;
        } else if ( sta == 1 ) {
            if( ub == 0x5A ) {
                sta = 2;
                ir = 0;

                frame.tm = tm_get_us();
            } else
                sta = 0;
        } else if ( sta == 2 ) {
            buf[ir++] = ub;
            if( ir == 1 ) frame_len = buf[0];

            if( ir >= frame_len ) {
                sta = 0;
                parse_frame(buf);
            }
        }
    }
}


int AHRS_Mini::parse_frame(uint8_t *buf)
{
    int             frame_len;
    AHRS_FrameType  frame_type;
    int16_t         temp;
    int             i;
    int             crc_sum;

    // frame length (in byte)
    frame_len = buf[0];

    // frame type
    if( buf[1] == 0xA1 )
        frame_type = AHRS_FRAME_AHRS;
    else if( buf[1] == 0xA2 )
        frame_type = AHRS_FRAME_SENSOR;

    // AHRS data
    if( frame_type == AHRS_FRAME_AHRS ) {
        temp = buf[2] << 8 | buf[3];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.yaw = 0.1*temp;

        temp = buf[4] << 8 | buf[5];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.pitch = 0.1*temp;

        temp = buf[6] << 8 | buf[7];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.roll = 0.1*temp;

        temp = buf[8] << 8 | buf[9];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.alt = 0.1*temp;

        temp = buf[10] << 8 | buf[11];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.temp = 0.1*temp;

        temp = buf[12] << 8 | buf[13];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.press = 10.0*temp;

        temp = buf[14] << 8 | buf[15];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.imu_ps = temp;

        frame.tm_a = frame.tm;

        frame.crc_a = buf[16];

        crc_sum  = 0;
        for(i=0; i<16; i++) crc_sum += buf[i];
        frame.crcv_a = crc_sum % 256;

        if( frame.crc_a == frame.crcv_a )
            frame.correct = 1;
        else
            frame.correct = 0;
    } else if ( frame_type == AHRS_FRAME_SENSOR ) {
        temp = buf[2] << 8 | buf[3];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.Ax = temp;

        temp = buf[4] << 8 | buf[5];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.Ay = temp;

        temp = buf[6] << 8 | buf[7];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.Az = temp;

        temp = buf[8] << 8 | buf[9];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.Gx = temp;

        temp = buf[10] << 8 | buf[11];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.Gy = temp;

        temp = buf[12] << 8 | buf[13];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.Gz = temp;

        temp = buf[14] << 8 | buf[15];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.Mx = temp;

        temp = buf[16] << 8 | buf[17];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.My = temp;

        temp = buf[18] << 8 | buf[19];
        if( temp & 0x8000 ) temp = 0 - (temp&0x7fff);
        else temp = temp & 0x7fff;
        frame.Mz = temp;

        frame.tm_s = frame.tm;

        frame.crc_s = buf[20];

        crc_sum  = 0;
        for(i=0; i<20; i++) crc_sum += buf[i];
        frame.crcv_s = crc_sum % 256;

        if( frame.correct == 1 && (frame.crc_s == frame.crcv_s) )
            frame.correct = 1;
        else
            frame.correct = 0;

        // push to frame queue
        mutex.lock();
        frameQueue.push_back(frame);
        mutex.unlock();
    }

    if( frame.correct ) return 0;
    else return -1;
}

int AHRS_Mini::frame_ready(void)
{
    if( frameQueue.size() > 0 )
        return 1;
    else
        return 0;
}

int AHRS_Mini::frame_num(void)
{
    return frameQueue.size();
}

int AHRS_Mini::frame_clear(void)
{
    mutex.lock();
    frameQueue.clear();
    mutex.unlock();

    return 0;
}

AHRS_Frame AHRS_Mini::frame_get(void)
{
    AHRS_Frame  f;

    if( frameQueue.size() > 0 ) {
        mutex.lock();
        f = frameQueue.front();
        frameQueue.pop_front();
        mutex.unlock();
    } else
        f.clear();

    return f;
}

int AHRS_Mini::frame_get(AHRS_Frame &f)
{
    if( frameQueue.size() > 0 ) {
        mutex.lock();
        f = frameQueue.front();
        frameQueue.pop_front();
        mutex.unlock();

        return 0;
    } else {
        f.clear();
        return 1;
    }
}

int AHRS_Mini::frame_get_last(AHRS_Frame &f)
{
    if( frameQueue.size() > 0 ) {
        mutex.lock();
        f = frameQueue.back();
        frameQueue.clear();
        mutex.unlock();

        return 0;
    } else {
        f.clear();
        return 1;
    }
}
